#ifndef READ_DATA_H
#define READ_DATA_H
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <stdio.h>
#include <map>
#include <queue>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <unordered_map>
#include <algorithm>
#include <memory>
#include <unordered_set>
#include <tuple>
#include <cmath>

#include <opencv2/opencv.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "mapping/sliding_window.h"

using namespace std;
using namespace mapping;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

inline std::string& ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), 
        [](int ch) { return !std::isspace(ch); }));
    return s;
}

// 去除右侧空白字符
inline std::string& rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), 
        [](int ch) { return !std::isspace(ch); }).base(), s.end());
    return s;
}

// 去除两侧空白字符
inline std::string& trim(std::string &s) {
    return ltrim(rtrim(s));
}

// 返回新字符串的版本（不修改原字符串）
inline std::string trim_copy(std::string s) {
    trim(s);
    return s;
}

class OriginFeaturePoint
{
public:
    int x;
    int y;
    float score;
    float descriptor_data[128];
    OriginFeaturePoint(const int _x, const int _y, const float _score = 1.0);
};

class FeatureOutput
{
public:
    struct Timestamp
    {
        int32_t sec;
        int32_t nsec;
    };

    Timestamp timestamp;
    std::vector<OriginFeaturePoint> fp_vector;
    float global_feature[256];
};

class read_data {
public:
    read_data(const std::string& path);
    void read_sensor_data(std::vector<ImuData>& imu_data_list, std::vector<WheelData>& wheel_data_list);
    void read_feature(std::vector<feature::FeaturePoint>& feature_data);
    Eigen::Matrix4d read_extrinsic();
    Eigen::Matrix3d read_intrinsic();
private:
    std::vector<std::string> split(std::string str, std::string pattern);

    std::string path_;
};

#endif


